Position and orientation detection system

ABSTRACT

A system for determining the position, orientation and deformation in 3 dimensional space of a moving object in real-time is provided having a plurality of activatable markers which are mounted onto the object and a sensor section comprising a plurality of individual sensors as shown diagrammatically in FIG.  1.  Individual markers are activated in sequence with only one marker energised at any time. Energy emitted by each activated marker is detected by the plurality of sensors positioned remotely. Since only one marker is activated at any time individual determination of the position of the particular energised marker is simplified. Individual sensors are arranged such that each sensor receives a different energy level from an energised marker depending on the relative position of the marker to the sensor. A control unit analyses the energy distribution range to determine the actual marker position. The system compensates for ambient energy and sensor saturation.

[0001] This invention generally relates to systems for determining the position and orientation of an object which may deform in shape over time and which use the detection of energy emitted from markers placed on the object.

[0002] As is known in the art, passive systems exist which rely on the markers being illuminated with energy that reflects off the markers and is detected by the sensor system. Active systems also exist in which the markers are individual sources of energy. In both cases the energy is focused onto spaced sensors, such that the position of an energised marker is identified by the sub set of adjacent sensor points that are recording an energy level above a given threshold. By identifying which adjacent sensors are detecting energy above the threshold, the associated computing devices can estimate the position of the marker emitting the energy in a given plane in space since the focusing function relates a point in space to a sensor on the sensor system. To achieve a high resolution measurement from such systems a very large number of sensors need to be positioned adjacent to each other as each sensor relates to a point in space. Having a large number of sensors leads to a degradation in the capture rate as the signal levels must be digitised for a large number of sensors. By using three displaced sensor sets, the position of the marker can be calculated to a certain level of accuracy in 3 dimensional space.

[0003] In such systems energy from the marker is directly focused onto the sensors so that only a small number of sensors detect energy over the given threshold. Such systems do not measure the distribution of energy levels across a large percentage of the total number of sensors and do not calculate the position of the marker based on an energy distribution function for which a maximum value occurs for a calculated marker position.

[0004] In passive systems all illuminated markers are energised and detected simultaneously. Therefore the computing device needs firstly to identify which sub set of adjacent sensors detecting energy above the given threshold correspond to which marker. Secondly it must track each marker from one sample to the next and attempt to distinguish each marker at all times. This results in the possibility of errors where marker assignments are lost and requires intensive processing methods.

[0005] Active systems may illuminate all markers at the same time or they can cycle the illumination of each marker to aid the computing system distinguish individual markers. If all markers illuminate at the same time, the computing device must be able to identify the correspondence of each marker and each energy detection and it must then track each marker in a similar way to the passive system.

[0006] In an active system that illuminates each marker individually, the computing device can immediately make the correspondence of marker energy emission and detection since the cycle time will be known. As each energy emission is recorded separately, no tracking is required and the position is simply calculated for each illumination. In one such system the sensor set is a multiple charge coupled device (CCD) onto which the energy is focused. To detect in 3D space at least three CCD detectors are used. In order to achieve high measurement resolution the CCD must have a large number of detecting sensors since the focusing function relates a point in space to each sensor point on the CCD. In order to achieve very high sample rates for a large number of markers the CCD must be driven at very high rates, well above the needs of conventional CCD devices. High resolution CCD devices capable of working at high data capture rates are expensive. In addition systems that use CCD devices have a measurement non-linearity dependent on the ability of the lens component to accurately focus the marker points linearly across the CCD sensor surface and not introduce any aberration.

[0007] As the CCD is moved further away from the markers on the object of interest, the measurement scaling changes since the focused image of the markers on the sensor system changes in size. Due to lens aberration and changes in measurement scaling such systems require a calibration phase in which an object of known dimensions is introduced for sampling. This calibration task is inconvenient, often needs experienced personnel to perform it and is considered a negative aspect from the point of view of end user.

[0008] In accordance with the present invention, a system for determining the position, orientation and deformation in 3 dimensional space of a moving object in real-time is provided having a plurality of activatable markers which are mounted onto parts of the object for which a position value needs to be recorded, a drive unit which drives the activatable markers in a defined sequence, a sensor section comprising a plurality of sensors remote from the markers and suitably arranged such that the energy falling on the sensor is dependent on the relative position of the energised marker and the sensor, a data capture unit which digitises the signals sensed by each sensor and a control unit which processes the signals received from the data capture unit.

[0009] The object may move in space and may deform in shape and the markers are mounted so as to move with the object and follow the shape deformation.

[0010] Each marker is activated individually for a period of time by the drive unit in a sequence known to the data capture unit and the control unit. While each marker is illuminated the energy from the marker is detected by all sensors. The sensors are arranged such that the energy distribution sensed by the plurality of markers for a single energised marker is a function of the position of the marker. The digitised energy levels are transmitted to the control unit at high speed and the information is processed to determine the position of the marker. The control unit calculates the position of the marker based on an energy distribution function for which a maximum value occurs for a calculated marker position.

[0011] By using this approach relatively few sensors are needed to determine marker position. This results in low digitisation and data collection overheads and therefore faster sample rates than if CCD devices were used. In addition by using a much lower number of sensor components significant cost reductions are achievable.

[0012] It is important to stress that the amplitude of the energy signal is not used to determine the position of the marker rather it is the energy distribution pattern over a number of sensors. For example the distance from the marker to a sensor is not calculated using the energy amplitude detected per se; in which case the emitter and sensor would need to be finely calibrated.

[0013] Since accurate signal strength values are unnecessary calibration of the energy emission and detection components is unnecessary.

[0014] Since the system relies on the way energy is distributed over the sensors, there is no need to calibrate the system for measurement scaling.

[0015] Since only one marker is activated at one time during a single cycle, the sensor section can individually determine the position of each marker as each marker is separately illuminated thereby making marker tracking unnecessary.

[0016] To reduce significantly the effect of external ambient energy radiation being superimposed upon the energy signal emitted by each marker the drive unit can split each specific marker illumination period into two parts. In the first part the marker is fully illuminated and the signal level detected is digitised. During a second part the marker is not illuminated and instead the data capture unit samples the ambient energy signal for each sensor and digitises it. Since the time interval between the two samples is very small the ambient energy level can be assumed to equal the subsequent ambient energy signal recorded and since the effect of ambient energy on marker energy detection can be considered to follow a simple superimposition rule, the final signal level attributed to the marker is the illuminated signal level less the ambient signal level. This subtraction can be performed by the data capture unit and the result can be transmitted to the control unit.

[0017] To deal with possible saturation of sensors due to the marker being too close to a group of sensors the drive unit can be designed to drive the marker with maximum illumination followed by a period of illumination at 50% of maximum, followed by the zero illumination level for the purposes of ambient energy detection. In cases where the maximum illumination level results in a saturation of the sensors, the data capture unit can globally choose to use the energy levels recorded during the time at which the drive unit illuminated the marker at 50% of maximum drive. In this way the system can automatically adapt to saturation of the sensors which may occur if the marker is positioned very close to the sensors in which case the signal values detected during the period of lower energy emission are used for the calculations.

[0018] In a preferred embodiment of the invention, the system includes: a plurality of infra red emitting markers that can be mounted to points of interest on the object; a drive unit which sequences the activation of the markers according to a synchronisation signal derived from the control unit; a sensor section, preferably a linear array of infra red sensors where each sensor is set approximately 75 mm apart and placed behind a linear collimator slot orthogonal to the linear array axis; a data capture section comprising a set of microprocessors which read the sensor levels and send the data to a control unit; a control unit comprising a processor to calculate the position of each marker and a data reception part for reception of the sensor level data from the data capture unit.

[0019] The sensors are provided with a collimator through which the energy may pass only up to a limited angle of incidence after which the sensor detects no energy. When the marker is directly positioned above a sensor the full energy of the marker is detected however as the marker moves away from the perpendicular axis adjoining the sensor, the energy level detected reduces as the collimator begins to attenuate the energy emitted by the marker.

[0020] The control unit activates the drive unit to begin the illumination of markers in a sequence known to the control unit. The sensor section detects the emitted energy and the data capture section digitises the levels and transmits them to the control unit. On reception the control unit analyses the data from a single sensor axis and calculates the most likely point along the length of the sensor axis which corresponds to the nearest point between the axis and the marker.

[0021] The control unit may receive data from several sensor axes, at least three, and will compute the 3D position of the marker in 3D space.

[0022] The control unit may display the information, store it or transmit it to another computing device for further use.

[0023]FIG. 1 is a block diagram of a system for determining the position in 3 dimensional space of a moving object in real-time according to the invention.

[0024]FIG. 2 is a diagram indicating how energy emitted from a marker in different positions falls onto a sensor masked with a slot collimator.

[0025]FIG. 3 is a graph depicting the intensity of energy received from each sensor arrangement where 5 sensor arrangements 1,2,3,4,5 are placed in a straight line and equidistant to each other.

[0026]FIG. 4a, 4 b, 4 c, are flow charts of the sub processes used by the system in FIG. 1 in determining the position in 3 dimensional space of a moving object in real-time according to the invention.

[0027]FIG. 4a is the flow chart for the synchronisation, reception and co-ordinate processing by the control unit. FIG. 4b is the flow chart for the sensor signal digitisation by the data capture unit. FIG. 4c is the drive sequence flow chart for the illumination of markers. T is the total period of drive time specified for one marker

[0028]FIG. 5 depicts the energy levels from a single sensor during a typical marker illumination sequence showing the levels recorded during maximum illumination, 50% of maximum and during ambient recording. T is the total period of drive time specified for one marker

[0029] Referring to FIG. 1 a system 1 for determining the position, orientation and deformation in three dimensional space of a moving object 2 is provided wherein a plurality of, in this instance five, active emitters or markers 3 a, 3 b, 3 c, 3 d, 3 e affixed to the object 2 are activated by a drive unit 4 following a set sequence, for example 3 a then 3 b then 3 c then 3 d then 3 e for each cycle, known to the control unit 5. The drive unit begins a drive cycle as a result of a synchronisation signal 6 sent by the control unit 5. As each marker is illuminated in turn by the drive unit, emitted energy from each marker is detected by a linear array of sensors 7 containing sensors 8 a, 8 b, 8 c, 8 d, 8 e, displaced by a distance depending on the resolution required but preferably 75 mm, and positioned behind collimating slots 9, which are arranged orthogonal to the array axis. The slots 9 a, 9 b, 9 c, 9 d, 9 e, act as barriers to energy arriving at an angle greater than Theta but pass energy arriving within this angle of incidence. As shown in FIG. 2, this arrangement ensures that a marker can be detected only if it is within a region of space running perpendicular to the linear array. It also ensures that the level of energy detected by adjacent sensors decreases on either side of the point along the length of the sensor axis which corresponds to the nearest point between the axis and the marker. The data capture unit 10, receives the synchronisation signal from the control unit and synchronises the digitisation of the energy levels detected by the sensors 8, and transmits them to the control unit 5. The control unit 5 calculates for each linear array and for each marker the position along the length of the sensor axis that corresponds to the nearest point between the axis and the marker.

[0030] The flow diagrams of FIG. 4a, 4 b and 4 c, explains the processes involved in the control unit, the data capture unit and the drive unit.

[0031] In the preferred embodiment, the calculation of the position of a marker relative to a linear sensor array is performed by the control unit 5. The control unit receives energy signal information from all sensors in the array corresponding to an activated marker. FIG. 3 is a graph depicting the energy distribution in a typical linear sensor array of 5 sensors. The control unit processor loads the signal level values from each sensor into an array in memory and determines which sensors are registering a signal level above zero. Depending on the sensor arrangement the energy distribution function can be described by linear, quasi-linear or non-linear equations. In one embodiment the energy distribution is parabolic in shape and therefore the processor computes the coefficients of a quadratic equation that will best fit the energy distribution data.

[0032] Having determined the equation coefficients, the estimate for the marker position is calculated as the point for which the equation evaluates to a maximum.

[0033] Having calculated the position of the marker relative for the particular sensor array, the control unit 5 loads data relating to the next sensory array and applies the same algorithm to determine the position of the marker relative to that sensor array. Once all sensor array data has been processed for the marker of concern, sensor data relating to the next marker in the sequence can be processed. If more than one processor is used in the control unit, this processing may be shared across processors.

[0034] Once all marker positions have been calculated the marker position data may be further processed according to the needs of the application.

[0035]FIG. 5 depicts the sensor level value for one sensor during the activation of a single marker in time. The figure shows at 1, the signal level recorded when the marker is illuminated at maximum energy output, and shows at 2, the signal level recorded when the marker is illuminated at 50% of maximum energy and shows at 3, the signal level recorded when the marker is not illuminated at all. The data capture unit 10 samples and digitises the levels at each point 1, 2 and 3 and stores these values. The capture unit 10 examines all sensor levels associated with the maximum energy emission timing and determines if any are higher than a level that would suggest at least one sensor was in saturation. If a single sensor is in saturation then the capture unit 10 will default to using only digitised sensor values recorded during the time that only 50% energy was emitted from the marker. In this way the capture unit can make a significant contribution to filtering data from saturated sensors which would degrade the marker position calculation accuracy.

[0036] In order to compensate for ambient radiation, the capture unit 10 will subtract from the saturation filtered energy signal, the level recorded during the time of zero illumination of the marker. Since the time interval between the two samples is very small, the ambient energy level can be assumed to equal the ambient energy present during the time that the marker was illuminated at either 100% or 50% of the maximum. Since the effect of ambient energy on marker energy detection can be considered to follow a simple superimposition rule, the final signal level attributed to the marker is the illuminated signal level less the ambient signal level. The capture unit 10 therefore performs the subtraction and stores the result for subsequent transmission to the control unit 5, for processing. 

1 A system for determining the position, orientation and deformation in 3 dimensional space of a moving object or objects in real time comprising: energy emitting markers placed on the object; signal generating means coupled to the markers, the signal generating means driving each marker in a specific sequence; sensors to detect the energy emitted by a marker; an arrangement of the sensors, the arrangement of the sensors contributing to detecting an energy distribution for a given energy emitting marker signal detection means coupled to the sensors to measure the energy detected by each sensor; processing means for sequencing the energy emission of each marker and processing the energy levels detected by each sensor so as to derive a position, orientation and deformation information which can be transmitted to another computing device for further use. 2 The system in claim 1 wherein a plurality of markers are attached to the surface of the moving object. 3 The system in claim 2 wherein the sensors are arranged in the proximity of the moving object. 4 The system of claim 1 wherein the signal generating means and the signal detection means are synchronised whereby the signal generating means energises a specific marker at a point in time and for a period of time known to the signal detection means and whereby the signal detection means will measure the energy level detected by each of the sensors and store the energy levels as digital values corresponding to the energy incident on each sensor that energy originating from the energised marker. 5 The system in claim 4 wherein the sensors and signal detection means are arranged such that the energy levels stored provide a set of digital data corresponding to the distribution of energy from an energised marker onto the sensors. 6 The system in claim 4 wherein the signal generating means and signal detection means are synchronised and will energise in sequence a plurality of markers whereby the signal detection means stores in separate locations the digital values of the energy level distribution corresponding to each of the plurality of markers and subsequently transmits them to the processing means for further use. 7 The system in claim 6 wherein the processing means receives from the signal detection means the set of digital values of the energy distribution levels corresponding to the plurality of markers energised in sequence. 8 The system in claim 7 wherein the processing means applies computational algorithms to the digital values whereby for a particular marker the values corresponding to the energy distribution across the sensors is used to calculate the position of the marker in relation to the sensors by predicting from the distribution values a point in space which corresponds to the point at which a maximum energy distribution would be detected. 9 The system in claim 8 wherein the processing means calculates the position of the each of the plurality of markers and stores the position for further use. 10 The system in claim 8 wherein the sensor arrangement comprises a plurality of equidistant sensors, arranged along a straight line, including such enclosure to act so that the energy from a marker detected by one sensor decreases as the marker moves along a line parallel with the line of equidistant sensors. 11 The system in claim 10 wherein the energy distribution values are used to predict the line in space perpendicular to the line of equidistant sensors corresponding to a maximum energy distribution and along which the marker is predicted to be present irrespective of the distance of the marker from the line of equidistant sensors. 12 The system in claim 11 wherein three or more arrangements of lines of equidistant sensors are used to determine three or more lines in space perpendicular to the corresponding line of equidistant sensors, the intersection of these three or more perpendicular lines being the calculated point in three dimensional space of the corresponding marker. 13 The system in claim 6 wherein there is a scheme to compensate for ambient energy changing at a much slower rate than the signal detection sequence rate whereby the signal generation drive to the marker energises the marker for an initial period with the full drive energy, this initial period then being followed by a second period of similar duration during which the marker is energised with no energy, these periods arranged in synchronisation with the signal detection means such that the signal detection means can detect, digitise and store the energy levels for full and no marker energy drive and subtract the energy level detected during the period where the marker was not energised from the energy level detected when the marker was fully energised, the resultant value corresponding to the energy detected from the marker compensated to a first order for ambient energy which is assumed to be changing very slowly compared to the signal detection rate. 14 The system in claim 6 wherein there is a scheme to compensate for the saturation of the signal detection means in the case the energy level arriving at a sensor saturates the sensor such as could occur if the marker is in close proximity to the sensors whereby the signal generation drive to the marker energises the marker for an initial period with the full drive energy, this initial period then being followed by a second period of similar duration during which the marker is energised with half the full energy, this second period then being followed by a third period of similar duration during which the marker is energised with no energy, these periods arranged in synchronisation with the signal detection means such that the signal detection means can detect, digitise and store the energy levels for full, half and no marker energy drive and determine if the energy level corresponding to the full energy drive period is saturated and in the case it is saturated the energy levels detected during the second period corresponding to a half energy level is used for all detectors, while in the case there is no saturation the energy levels detected during the first period corresponding to a full energy level is used for all detectors and as described in claim 6 from the selected energy level the signal detection means will subtract the energy level detected during the period where the marker was not energised, the resultant value corresponding to the energy detected from the marker compensated to a first order for ambient energy which is assumed to be changing very slowly compared to the signal detection rate and for possible saturation of the sensors of the signal detection means. 15 The system in claim 10 wherein the enclosure includes a flat panel placed in between the plurality of markers and the line of equidistant sensors and the flat panel being placed at a fixed perpendicular distance away from each sensor, the panel being impervious to the marker energy thereby blocking the energy arriving at all sensors, wherein the flat panel includes for each sensor, at a height directly above each sensor, a cut out slot which allows energy to pass through the panel, the slot having a width similar to the sensor width and length similar to the height of the slot above the sensor, the slots length running orthogonal to the line of equidistant sensors, the effect of the slot being to allow energy from a marker placed at a fixed perpendicular distance from the panel to be modified in strength, due to the constraining effect of the slot on the angle of incidence, this modification in strength occurring predominantly when the marker is moved along a line parallel to the line of equidistant sensors, with a maximum occurring for a specific sensor when the marker is directly above slot and sensor, and resulting in an energy distribution over the plurality of sensors. 16 The system in claim 8 wherein the processing means uses a mathematical function to describe the energy distribution and predicts the position of the marker as corresponding to where there is a maxima in the function wherein the distribution is modelled either as a linear or on-linear function such as a weighted distance average or a quadratic function in which cases the weights correspond to the energy levels, the distance being the accumulated distance of the distance between sensors, the distance average being the average of the product of weights and distances for each sensor, the argument of the non-linear function being distance and the function value being the predicted energy level. 